Randomized searches and nonlinear programming in trajectory planning

This paper presents a novel trajectory planning algorithm for nonlinear dynamical systems evolving in environments with complex obstacles. The incremental search algorithm entails (i) a global exploration strategy based on randomization and on a rapidly-exploring heuristic, and (ii) a local planner based on collocation and nonlinear programming. To numerically validate the design, we consider a six degree of freedom vehicle model subject to saturation limits on the control inputs and obstacles on the state variables. Experimental results indicate that the proposed scheme outperforms implementations based solely on nonlinear programming or on randomization.